#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <cvctracker/cvc_tracker.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include <ctype.h>
#include <GL/glew.h>
#define FREEGLUT_STATIC
#include <freeglut/GL/freeglut.h>
#include "render.h"


using namespace cv;
using namespace std;

enum detecFeature {CHESSBOARD, NATURAL};
bool isNaturalAvalible = false;
int detecFeatureMode = CHESSBOARD;
bool enhancedMatching = false;
Mat img_matches;

VideoCapture cap;
Mat frame;
Mat gray, prevGray, image;

double proj[16];
double mv[16];
double mvInv[16];
double cameraPos[3];
double cameraRot[9];

cv::SurfFeatureDetector detector( 600 );
cv::SurfDescriptorExtractor extractor;
cv::DescriptorMatcher* matcher  = new cv::BFMatcher( cv::NORM_L2, true );
CVCTracker cvct( &detector , &extractor , matcher );
bool realtime = true;

double _radius = 700, _latitude = 89, _azimute = 00;
Renderer _render;


void printmat( cv::Mat M )
{
    for( int i = 0; i < M.rows; i++ )
    {
        const double* Mi = M.ptr<double>( i );
        for( int j = 0; j < M.cols; j++ )
            printf( " %lf ", Mi[j] );
        printf( "\n" );
    }
    printf( "\n" ); printf( "\n" );
}


void renderCube( float size )
{
    glBegin( GL_QUADS );
    // Front Face
    glNormal3f( 0.0f, 0.0f, 1.0f );
    glVertex3f( 0.0f,  0.0f,  0.0f );
    glVertex3f( size,  0.0f,  0.0f );
    glVertex3f( size,  size,  0.0f );
    glVertex3f( 0.0f,  size,  0.0f );
    // Back Face
    glNormal3f( 0.0f, 0.0f, -1.0f );
    glVertex3f( 0.0f,  0.0f, size );
    glVertex3f( 0.0f,  size, size );
    glVertex3f( size,  size, size );
    glVertex3f( size,  0.0f, size );
    // Top Face
    glNormal3f( 0.0f, 1.0f, 0.0f );
    glVertex3f( 0.0f,  size,  0.0f );
    glVertex3f( size,  size,  0.0f );
    glVertex3f( size,  size, size );
    glVertex3f( 0.0f,  size, size );
    // Bottom Face
    glNormal3f( 0.0f, -1.0f, 0.0f );
    glVertex3f( 0.0f,  0.0f,  0.0f );
    glVertex3f( 0.0f,  0.0f, size );
    glVertex3f( size,  0.0f, size );
    glVertex3f( size,  0.0f,  0.0f );
    // Right face
    glNormal3f( 1.0f, 0.0f, 0.0f );
    glVertex3f( size,  0.0f, 0.0f );
    glVertex3f( size,  0.0f, size );
    glVertex3f( size,  size, size );
    glVertex3f( size,  size, 0.0f );
    // Left Face
    glNormal3f( -1.0f, 0.0f, 0.0f );
    glVertex3f( 0.0f,  0.0f, 0.0f );
    glVertex3f( 0.0f,  size, 0.0f );
    glVertex3f( 0.0f,  size, size );
    glVertex3f( 0.0f,  0.0f, size );
    glEnd();
}

void renderAxis( float size )
{
    glBegin( GL_LINES );
    glColor3f( 1.0, 0.0, 0.0 );
    glVertex3f( 0.0f, 0.0f, 0.0f );
    glVertex3f( size, 0.0f, 0.0f );
    glColor3f( 0.0, 1.0, 0.0 );
    glVertex3f( 0.0f, 0.0f, 0.0f );
    glVertex3f( 0.0f, size, 0.0f );
    glColor3f( 0.0f, 0.0f, 1.0f );
    glVertex3f( 0.0f, 0.0f, 0.0f );
    glVertex3f( 0.0f, 0.0f, size );
    glEnd();
}

void glGetModelViewMatrix( cv::Mat&  Rvec , cv::Mat& Tvec, double modelview_matrix[16] )
{

    //coordinate system returned is relative to the first 3D input point
    for( int f = 0; f < 3; f++ )
    {
        for( int c = 0; c < 3; c++ )
        {
            modelview_matrix[c * 4 + f] = Rvec.ptr<double>()[f * 3 + c];	//transposed
            if( f > 0 )
            {
                modelview_matrix[c * 4 + f] *= -1;
            }
        }
    }


    modelview_matrix[3] = 0.0;
    modelview_matrix[7] = 0.0;
    modelview_matrix[11] = 0.0;
    modelview_matrix[12] = Tvec.at<double>( 0, 0 );
    modelview_matrix[13] = -Tvec.at<double>( 1, 0 );
    modelview_matrix[14] = -Tvec.at<double>( 2, 0 );
    modelview_matrix[15] = 1.0;


}

void getCameraPosition( double m[16], double mI[16], double trans[3], double rot[9] )
{
    cv::Mat modelview = ( cv::Mat_<double>( 4, 4 ) <<   m[0], m[1], m[2], m[3],   m[4], m[5], m[6], m[7],   m[8], m[9], m[10], m[11],   m[12], m[13], m[14], m[15] );
    cv::Mat modelviewinv = modelview.inv();

    memcpy( mI, modelviewinv.ptr<double>(), sizeof( double ) * 16 );
    //TODO retirar ate o final da funcao
    //printf( "\n m \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n", m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14], m[15] );
    //printf( "\n mI \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n", mI[0], mI[1], mI[2], mI[3], mI[4], mI[5], mI[6], mI[7], mI[8], mI[9], mI[10], mI[11], mI[12], mI[13], mI[14], mI[15] );
    //printf(" %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n",ptr[0],ptr[1],ptr[2],ptr[3],ptr[4],ptr[5],ptr[6],ptr[7],ptr[8],ptr[9],ptr[10],ptr[11],ptr[12],ptr[13],ptr[14],ptr[15]);

    //printf( "\nModelView\n" );
    //printmat( modelview );

    //printf( "\nModelView invertida\n" );
    //printmat( modelviewinv );
    //printf( "%lf %lf %lf\n", modelviewinv.at<double>( 3, 0 ), modelviewinv.at<double>( 3, 1 ), modelviewinv.at<double>( 3, 2 ) );
    trans[0] = modelviewinv.at<double>( 3, 0 );
    trans[1] = modelviewinv.at<double>( 3, 1 );
    trans[2] = modelviewinv.at<double>( 3, 2 );

    //rotacao da camera em coordenadas do mundo por linhas
    rot[0] = m[0]; rot[3] = m[4]; rot[6] = m[8];
    rot[1] = m[1]; rot[4] = m[5]; rot[7] = m[9];
    rot[2] = m[2]; rot[5] = m[6]; rot[8] = m[10];

    //printf( "\n rot \n %f %f %f \n %f %f %f \n %f %f %f \n ", rot[0], rot[1], rot[2], rot[3], rot[4], rot[5], rot[6], rot[7], rot[8] );

}

void DisplaySecondViewPort()
{
    glViewport( 640, 0, 640, 480 );
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    gluPerspective( 90, ( ( double )640 ) / ( ( double )480 ), 0.01, 10000 );
    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();

    double x = _radius * sin( _latitude * M_PI / 180 ) * sin( _azimute * M_PI / 180 );
    double y = _radius * cos( _latitude * M_PI / 180 );
    double z =  _radius * sin( _latitude * M_PI / 180 ) * cos( _azimute * M_PI / 180 );

    gluLookAt( x, y, z, 0, 0, 0, 0, 1.0, 0 );

    glColor4f( 0.0, 0.0, 1.0, 1.0 );
    glPointSize( 10 );
    glLineWidth( 5 );
    glBegin( GL_LINES ) ;
    glColor4f( 1.0, 0.0, 0.0, 1.0 );
    glVertex3d( 0, 0, 0 ) ;
    glVertex3d( 100, 0, 0 ) ;
    glColor4f( 0.0, 1.0, 0.0, 1.0 );
    glVertex3d( 0, 0, 0 ) ;
    glVertex3d( 0, 100, 0 ) ;
    glColor4f( 0.0, 0.0, 1.0, 1.0 );
    glVertex3d( 0, 0,  0 ) ;
    glVertex3d( 0, 0,  100 ) ;
    glEnd() ;
    glLineWidth( 1 );

    glBegin( GL_LINES ) ;
    glColor4f( 2.0, 2.0, 1.0, 1.0 );
    for( int i = 0 ; i < 50; i++ ) // xz
    {
        glVertex3d( ( i - 25 ) * 20,   0, -300 ) ;
        glVertex3d( ( i - 25 ) * 20,  0, 300 ) ;
    }
    glColor4f( 0.0, 1.0, 1.0, 1.0 );
    for( int i = 0 ; i < 20; i++ ) // yz
    {
        glVertex3d( 0, ( i - 10 ) * 40,    -300 ) ;
        glVertex3d( 0, ( i - 10 ) * 40,   300 ) ;
    }


    glEnd() ;

    glMultMatrixd( mvInv );
    renderAxis( 30 );
    glColor4f( 1.0, 0.0, 0.0, 1.0 );
    glutWireSphere( 5.0, 5, 5 );
    glTranslated( 0, 0, -100 );
    glColor4f( 0.0, 0.0, 1.0, 1.0 );
    glutWireSphere( 5.0, 5, 5 );





}

static void DisplayPrimeiroViewPort()
{
    glClear( GL_DEPTH_BUFFER_BIT );
    glViewport( 0, 0, 640, 480 );
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    glLoadMatrixd( proj );
    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();
    glColor4f( 1.0, 1.0, 1.0, 1.0 );
    glMultMatrixd( mv );
    renderAxis( 30 );
    renderCube( 30 );
}


static void Display( void )
{
    // the curve
    glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT );
    glViewport( 0, 0, 640, 480 );
    //glMatrixMode( GL_PROJECTION );
    //glLoadIdentity();
    //gluOrtho2D( 0.0, 640, 0.0,  480 );
    //glMatrixMode( GL_MODELVIEW );
    //glLoadIdentity();
    //glRasterPos2i( 0, 0 );


    if( image.data != NULL )
    {
        //Mat rotation, translation;

        //glDrawPixels( 640, 480, GL_RGB, GL_UNSIGNED_BYTE, ( GLvoid* )image.data );
        //cvct.getGLViewMatrix( mv );
        //_render.setMv( mv );
		float mvp[16];
		cvct.getModelViewProjection( mvp );
		_render.setMVP( mvp );
        _render.setBackgroundImage( ( const char* )image.data, 640, 480 );
        _render.render();

        //cvct.getGLMatrix(GlProject, rotation, translation);
        //	if (translation.data != NULL)
        {
            //	glGetModelViewMatrix(rotation,translation,mv);
            //cvct.getGLViewMatrix( mv );
            getCameraPosition( mv, mvInv, cameraPos, cameraRot );


            //DisplayPrimeiroViewPort();
            //DisplaySecondViewPort();

        }
    }
    glutSwapBuffers();
}



static void Mouse( int x , int y )
{
    //if (button == GLUT_LEFT_BUTTON )
    {
        if( x > 640 )
        {
            if( y > 460 )
            {
                _azimute = ( ( x - 640.0 ) / 640.0 ) * 360;
            }
            else if( x > 1260 )
            {
                _latitude = ( ( 480 - y ) / 460.0 ) * 180;
            }
        }
    }

}

static void MouseWheel( int w , int dir , int x , int y )
{
    _radius += dir * 10;

}

static void
Key( unsigned char key, int x, int y )
{
    int need_redisplay = 1;

    switch( key )
    {
        case 27:  /* Escape key */
            exit( 0 );
            break;

        case 't':
            realtime = ( realtime ) ? false : true;
            break;
        case 'l':
            frame = imread( "currentImage.png" );
            break;
        case 'r':
            imwrite( "currentImage.png", frame );
            break;
        case 'n' :
            cvct.useNaturalMarker( true );
            break ;
        case 'c' :
            cvct.useNaturalMarker( false );
            break ;
        case 's' :
            cvct.restartKLT();
            break ;
        default:
            need_redisplay = 0;
            break;
    }
    if( need_redisplay )
        glutPostRedisplay();
}


static void
Reshape( int width, int height )
{
    _render.init();
    glShadeModel( GL_SMOOTH );							// Enable Smooth Shading
    glClearColor( 0.0f, 0.0f, 0.0f, 0.5f );				// Black Background
    glClearDepth( 1.0f );									// Depth Buffer Setup
    glEnable( GL_DEPTH_TEST );							// Enables Depth Testing
    glDepthFunc( GL_LEQUAL );								// The Type Of Depth Testing To Do
    glEnable( GL_COLOR_MATERIAL );
    glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
    cvct.getGLProjectionMatrix( proj );
    _render.setProj( proj );
}

void Idle()
{
    Mat glProj, glMv;


    if( realtime )
    {
        cap >> frame;
    }

    cvtColor( frame, image, CV_BGR2RGB );
    cvtColor( frame, gray, CV_BGR2GRAY );

    cvct.faztudo( prevGray, gray );
    cvct.drawReprojection( image );
    swap( prevGray, gray );



    if( !frame.empty() && !image.empty() )
    {
        cv::flip( image, image, 0 );
        glutPostRedisplay();
    }
}

int
main( int argc, char* argv[] )
{
    int fractal_window ;
    cvct.useKLT( true );
    cvct.useNaturalMarker( true );

    TermCriteria termcrit( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 );
    Size subPixWinSize( 10, 10 ), winSize( 31, 31 );

    const int MAX_COUNT = 500;
    bool needToInit = false;
    bool nightMode = false;

    if( argc == 1 || ( argc == 2 && strlen( argv[1] ) == 1 && isdigit( argv[1][0] ) ) )
        cap.open( argc == 2 ? argv[1][0] - '0' : 0 );
    else if( argc == 2 )
        cap.open( argv[1] );

    if( !cap.isOpened() )
    {
        cout << "Could not initialize capturing...\n";
        return 0;
    }

#if 1
    isNaturalAvalible = cvct.loadMarkerImage( "d:\\android16\\temp\\obidosRR.png" );
#else
    cv::Mat imageFile = cv::imread( "d:\\android16\\temp\\obidosRR.png" );
    if( imageFile.data == NULL )
    {
        isNaturalAvalible =  false;
    }
    else
        cvct.loadMarkerImage( imageFile );
#endif


    glutInitWindowSize( 1280, 480 );
    glutInitWindowPosition( 140, 140 );
    glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE );
    glutInit( &argc, argv );


    fractal_window = glutCreateWindow( "CVCTrackerTestGL" );

    glClearColor( 1.0, 1.0, 1.0, 1.0 );

    glutReshapeFunc( Reshape );
    glutIdleFunc( Idle );
    glutKeyboardFunc( Key );
    //glutMouseFunc(Mouse);
    glutMotionFunc( Mouse );
    glutMouseWheelFunc( MouseWheel );
    //glutSpecialFunc(Special);
    glutDisplayFunc( Display );

#ifdef WIN32
#endif

    //while ( true )
    glutMainLoop();

    printf( "Back from the 'freeglut' main loop\n" ) ;

    return 0;             /* ANSI C requires main to return int. */
}

